//
// Created by tatyx on 2025/8/26.
//
#include "DM_G6220.h"
#include <string.h>


FDCAN1_TxFrame_TypeDef hfdcan2_TxFrame_DM = {
    .hcan   = &hfdcan2,
    .Header.IdType = FDCAN_STANDARD_ID,
    .Header.TxFrameType = FDCAN_DATA_FRAME,
    .Header.DataLength = FDCAN_DLC_BYTES_8,
    .Header.ErrorStateIndicator = FDCAN_ESI_ACTIVE,
    .Header.BitRateSwitch = FDCAN_BRS_OFF,
    .Header.FDFormat = FDCAN_CLASSIC_CAN,
    .Header.TxEventFifoControl = FDCAN_NO_TX_EVENTS,
    .Header.MessageMarker = 0
  };

void DM_FDCAN_TxData_8byteS(FDCAN2_TxFrame_TypeDef *TxFrame, u8 *data, u8 motorID)
{

    TxFrame->Header.Identifier = motorID;
    TxFrame->Data[0] = data[0];
    TxFrame->Data[1] = data[1];
    TxFrame->Data[2] = data[2];
    TxFrame->Data[3] = data[3];
    TxFrame->Data[4] = data[4];
    TxFrame->Data[5] = data[5];
    TxFrame->Data[6] = data[6];
    TxFrame->Data[7] = data[7];

    HAL_FDCAN_AddMessageToTxFifoQ(TxFrame->hcan, &TxFrame->Header,TxFrame->Data);
}
//🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊
void DM_Enable_motor(DM_TypeDef *motor )
{
    u8 data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc};
    DM_FDCAN_TxData_8byteS(&hfdcan2_TxFrame_DM, data, motor->motorID);
}
//🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊
void DM_DisEnable_motor(DM_TypeDef *motor)
{
    u8 data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd};
    DM_FDCAN_TxData_8byteS(&hfdcan2_TxFrame_DM, data, motor->motorID);
}
//🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊
void DM_Keep_zero(DM_TypeDef *motor)
{
    u8 data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe};
    DM_FDCAN_TxData_8byteS(&hfdcan2_TxFrame_DM, data, motor->motorID);
}
//🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊🍉🍓🍊
void DM_Clear_error(DM_TypeDef *motor)
{
    u8 data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe};
    DM_FDCAN_TxData_8byteS(&hfdcan2_TxFrame_DM, data, motor->motorID);
}

float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
   /// converts unsigned int to float, given range and number of bits ///
   float span = x_max - x_min;
   float offset = x_min;
   return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
int float_to_uint(float x, float x_min, float x_max, int bits)
{
   /// Converts a float to an unsigned int, given range and number of bits

    float span = x_max - x_min;
    float offset = x_min;
    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}




void DM_Mode_mit(DM_TypeDef *motor, float _pos, float _vel, float _KP, float _KD, float _torq)
{
    uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
    pos_tmp = float_to_uint(_pos, -12.5, 12.5, 16);
    vel_tmp = float_to_uint(_vel, -31, 31, 12);
    kp_tmp = float_to_uint(_KP, 0, 500, 12);
    kd_tmp = float_to_uint(_KD, 0, 5, 12);
    tor_tmp = float_to_uint(_torq, -2.7, 2.7, 12);
    u8 data[8];
    data[0] = (pos_tmp >> 8);
    data[1] = pos_tmp;
    data[2] = (vel_tmp >> 4);
    data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
    data[4] = kp_tmp;
    data[5] = (kd_tmp >> 4);
    data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
    data[7] = tor_tmp;
    DM_FDCAN_TxData_8byteS(&hfdcan2_TxFrame_DM, data, motor->motorID);
}
extern DM_TypeDef DM_6220;
void DM_Data_deal(FDCAN2_RxFrame_TypeDef *RxFrame)
{
    if (RxFrame->Header.Identifier==0x5F)
    {
        {
            u16 p_int=(RxFrame->Data[1]<<8)|RxFrame->Data[2];
            u16 v_int=(RxFrame->Data[3]<<4)|(RxFrame->Data[4]>>4);
            u16 t_int=((RxFrame->Data[4]&0xF)<<8)|RxFrame->Data[5];
            DM_6220.position = uint_to_float(p_int, -12.5, 12.5, 16); // (-12.5,12.5)
            DM_6220.velocity = uint_to_float(v_int, -31, 31, 12); // (-45.0,45.0)
            DM_6220.Torque = uint_to_float(t_int, -2.7, 2.7, 12); // (-2.7, 2.7)
            }
    }
}

_PID_arg_st speed_arg;
_PID_val_st speed_val;
void DM6220_speed_mit(float Speed)
{
    speed_arg.kp = 0.017;
    speed_arg.kd_ex = 0.00;
    speed_arg.ki = 0.0;
    PID_calculate( 0.0015,     //周期（单位：秒）
                    0,				//前馈值
                    Speed,				//期望值（设定值）
                    DM_6220.velocity,	//反馈值（）
                    &speed_arg, //PID参数结构体
                    &speed_val,	//PID数据结构体
                    0.001,//积分误差限幅
                    0.001//integration limit，积分限幅
                    );
    float aim_torque = _constrain(speed_val.out,-0.3,0.3);//范围限制
    DM_Mode_mit(&DM_6220, 0, 0, 0, 0, aim_torque);
     send_float_frame(DM_6220.velocity);
    // send_float_frame(DM_6220.Torque);
}

_PID_arg_st position_arg;
_PID_val_st position_val;
void DM6220_postion_mit(float position, float speed)
{
   position_arg.kp = 7;
   position_arg.kd_ex = 0.00000;
   position_arg.ki = 0.0;
    PID_calculate( 0.0014,     //周期（单位：秒）
                    0,				//前馈值
                     position,				//期望值（设定值）
                    DM_6220.position,	//反馈值（）
                    &position_arg, //PID参数结构体
                    &position_val,	//PID数据结构体
                    0.00,//积分误差限幅
                    0.00//integration limit，积分限幅
                    );
    float aim_speed = _constrain(position_val.out,-speed,speed);//范围限制
    DM6220_speed_mit(aim_speed);
    send_float_frame(DM_6220.position);
}